#include "common.h"

#define NUM_NEED 4
#define FUNC_OFFSET (3 * sizeof(int))

/* todo
 * use down or down_read, down_write to use sem to protect
 */ 

static unsigned int saved_st[NUM_NEED];
static void *target_func;

extern int setup_arg_pages(
    struct linux_binprm *bprm,
    unsigned long stack_top,
    int executable_stack);

static void clean_cache(void *p1, void *p2)
{
    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c14, 1\n"        
        ::"r"(p1));
    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c14, 1\n"        
        ::"r"(p2));    

    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c5, 0\n"
        ::"r"(0));
}

static int hijack_func(void *func);
int my_func(
    struct linux_binprm *bprm,
    unsigned long stack_top,
    int executable_stack)
{
    int ret;
    memcpy(target_func, saved_st, 4 * NUM_NEED);
    clean_cache(target_func, target_func + NUM_NEED);

    printk("bprm->p = %lx\n", bprm->p);
    printk("stack_top = %lx, executable_stack = %d\n", stack_top, executable_stack);
    ret = setup_arg_pages(bprm, 0xbed7a000, executable_stack);
    printk("bprm->p = %lx\n", bprm->p);
    
	hijack_func(target_func);    
    return (ret);
}

static void jmpfunc(void)
{
    __asm__ __volatile__ ("mov %%pc, %0"
                          ::"r"((void *)my_func));    
}

static int hijack_func(void *func)
{
    void *pfunc, *pjmpfunc;

    pfunc = (void *)func;
    pjmpfunc = (void *)jmpfunc;

    memcpy(func, pjmpfunc + FUNC_OFFSET, sizeof(int) * NUM_NEED);

    clean_cache((void *)func, (void *)func + NUM_NEED);
    return (0);
}



static int save_old(void *func)
{
    memcpy(saved_st, func, sizeof(int) * NUM_NEED);
    return (0);
}

int teststack_init(void)
{
    target_func = setup_arg_pages;
    printk(KERN_DEBUG "teststack init\n");
    save_old(target_func);
    hijack_func(target_func);
    return 0;
}

void teststack_exit(void)
{
    memcpy((void *)target_func, saved_st, 4 * NUM_NEED);
    clean_cache((void *)target_func, (void *)target_func + NUM_NEED);    
	printk(KERN_DEBUG "teststack exit\n");
}

module_init(teststack_init);
module_exit(teststack_exit);


MODULE_LICENSE("GPL");
MODULE_AUTHOR("tpl.a1406@gmail.com");


